#include "Pathfinding.h"
#include <algorithm>

namespace Engine
{
	Pathfinding::Pathfinding()
	{
		m_startNode.reset();
		m_goalNode.reset();

		m_goalFound = false;

		m_openList.reserve(
			TheWorld::Instance()->GetWorldWidth() *
			TheWorld::Instance()->GetWorldHeight());
	}

	bool Pathfinding::AStar(Vec2 startPos, Vec2 targetPos)
	{
		bool result;
		result = FindPath(ToGridCoords(startPos), ToGridCoords(targetPos));

		return result;
	}

	bool Pathfinding::FindPath(Vec2 currentPos, Vec2 targetPos)
	{
		if(m_goalFound)
		{
			m_openList.clear();
			m_closedList.clear();
			m_path.clear();
		}

		m_startNode.reset(
			new PathNode(currentPos.x, currentPos.y,
			currentPos.x + currentPos.y * TheWorld::Instance()->GetWorldWidth(),
			0));

		m_goalNode.reset(
			new PathNode(targetPos.x, targetPos.y,
			targetPos.x + targetPos.y * TheWorld::Instance()->GetWorldWidth(),
			0));

		m_startNode->m_h = m_startNode->Heuristic(m_goalNode.get());
		m_openList.push_back(m_startNode);
		m_goalFound = false;

		SearchPath();

		return m_goalFound;
	}

	std::shared_ptr<PathNode> Pathfinding::GetNextNode()
	{
		std::make_heap(
			m_openList.begin(), m_openList.end(),
			[] (SPpn a, SPpn b)
			{
				return a->GetF() > b->GetF();
			});

		SPpn nextNode = m_openList.front();
		m_closedList.push_back(nextNode);
		m_openList.erase(m_openList.begin());

		return nextNode;
	}

	void Pathfinding::AddNode(int x, int y, float newGcost, PathNode* _parent)
	{
		//if(!( check gif in grid boundary
		if(!TheWorld::Instance()->IsBoundary(x, y))
			return;

		int id = y * TheWorld::Instance()->GetWorldWidth() + x;

		// check if id is a wall here
		if(!TheWorld::Instance()->IsPassable(x, y))
			return;

		// ---> check if in the closed list
		for(auto it = m_closedList.cbegin();
			it != m_closedList.cend();
			it++)
		{
			if(id == (*it)->m_id)
			{
				return;
			}
		}

		// ---> check if in the openlist
		for(auto it = m_openList.cbegin();
			it != m_openList.cend();
			it++)
		{
			if(id == (*it)->m_id)
			{
				if((*it)->m_g <= newGcost)
				{
					return;
				}
				else
				{
					m_openList.erase(it);
					break;
				}
			}
		}

		SPpn newChild(new PathNode(x, y, id, _parent));

		newChild->m_g = newGcost;
		newChild->m_h = newChild->Heuristic(m_goalNode.get());
		m_openList.push_back(newChild);
	}

	void Pathfinding::SearchPath()
	{
		while(!m_openList.empty())
		{
			SPpn currentNode = GetNextNode();

			if(currentNode->m_id == m_goalNode->m_id)
			{
				m_goalNode->m_parent = currentNode->m_parent;
				PathNode* getPath = m_goalNode.get();

				while(getPath->m_parent != NULL)
				{
					m_path.push_back(SPv2(new Vec2));
					m_path.back()->x = getPath->m_x;
					m_path.back()->y = getPath->m_y;
					getPath = getPath->m_parent;
				} // ---> while parent

				m_goalFound = true;
				return;
			} // ---> if curr == goal
			else
			{
				//right
				AddNode(currentNode->m_x + 1, currentNode->m_y, currentNode->m_g + 1, currentNode.get());
				//left
				AddNode(currentNode->m_x - 1, currentNode->m_y, currentNode->m_g + 1, currentNode.get());
				//up
				AddNode(currentNode->m_x, currentNode->m_y - 1, currentNode->m_g + 1, currentNode.get());
				//down
				AddNode(currentNode->m_x, currentNode->m_y + 1, currentNode->m_g + 1, currentNode.get());
				//left up diag
				AddNode(currentNode->m_x - 1, currentNode->m_y - 1, currentNode->m_g + 1.414f, currentNode.get());
				//right up diag
				AddNode(currentNode->m_x + 1, currentNode->m_y - 1, currentNode->m_g + 1.414f, currentNode.get());
				//left down diag
				AddNode(currentNode->m_x - 1, currentNode->m_y + 1, currentNode->m_g + 1.414f, currentNode.get());
				//right down diag
				AddNode(currentNode->m_x + 1, currentNode->m_y + 1, currentNode->m_g + 1.414f, currentNode.get());
			}
		} // ---> while(){}

		m_goalFound = false;
		m_openList.clear();
		m_closedList.clear();
	}

	Vec2 Pathfinding::NextPathPos()
	{
		Vec2 nextPos = Vec2(0.0f, 0.0f);

		nextPos = (*m_path.back());
		m_path.pop_back();

		if(m_path.empty())
		{
			m_openList.clear();
			m_closedList.clear();
			m_goalFound = false;
		}

		return nextPos;
	}

	Vec2 Pathfinding::ToGridCoords(Vec2 worldCoords)
	{
		float x = worldCoords.x - (-TheWorld::Instance()->GetOffset().x);
		float y = worldCoords.y - (-TheWorld::Instance()->GetOffset().y);

		Vec2 result = Vec2(
			std::floor(std::abs(x / TheWorld::Instance()->GetCellWidth())),
			std::floor(std::abs(y / TheWorld::Instance()->GetCellHeight())));

		return result;
	}

	Vec2 Pathfinding::ToWorldCoords(Vec2 gridCoords)
	{
		float x = (-TheWorld::Instance()->GetOffset().x) + 
			(gridCoords.x * TheWorld::Instance()->GetCellWidth()) +
			(TheWorld::Instance()->GetCellWidth() * 0.5f);

		float y = (-TheWorld::Instance()->GetOffset().y) + 
			(gridCoords.y * TheWorld::Instance()->GetCellHeight()) +
			(TheWorld::Instance()->GetCellHeight() * 0.5f);

		Vec2 result = Vec2(x, y);

		return result;
	}
}